#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdint.h>
#include <stdbool.h>
#include <math.h>
#include <ctype.h>

#include "esp_system.h"
#include "driver/uart.h"
#include "driver/gpio.h"
#include "esp_log.h"
#include "esp_timer.h"
#include "motion.h"
#include "pin_def.h"

static const char *TAG = "motion";
typedef struct {
    // 零点位置
    int arm4_zero;
    int arm2_zero;
    int offset_finger1_zero;
    int offset_finger3_zero;
    
    // 位置参数（已转换为编码器值）
    int finger_clamp;
    int finger_init;
    int finger_flip_wait;
    int finger_flip;
    int finger_max;
    
    // 电流参数
    int max_current;
    int clamp_current;
    
    // 速度加速度参数
    int v_flip;
    int a_flip;
    int v_twist;
    int a_twist;
    int v_no_load;
    int a_no_load;
    int v_finger;
    int a_finger;
    
    // 联动参数
    int v_no_load_20_70_deg;
    int finger_no_load_start_arm;
    // 超时参数（us）
    int arm_motion_time_out;
} motion_cfg;

// 全局配置实例
static motion_cfg mcfg;

// 加载运动配置（从文件）
int load_motion_cfg(const char *path) 
{    
    FILE *file = fopen(path, "r");
    if (!file) {
        ESP_LOGE(TAG, "无法打开配置文件: %s", path);
        return -1;
    }

    // 定义所有配置项的标记
    typedef struct {
        bool arm4_zero;
        bool arm2_zero;
        bool offset_finger1_zero;
        bool offset_finger3_zero;
        bool finger_clamp;
        bool finger_init;
        bool finger_flip_wait;
        bool finger_flip;
        bool finger_max;
        bool max_current;
        bool clamp_current;
        bool speed_factor;
        bool v_flip;
        bool a_flip;
        bool v_twist;
        bool a_twist;
        bool v_no_load;
        bool a_no_load;
        bool v_finger;
        bool a_finger;
        bool v_no_load_20_70_deg;
        bool finger_no_load_start_arm;
        bool arm_motion_time_out;
    } ConfigFlags;
    
    ConfigFlags flags = {0};
    char line[256];
    int line_num = 0;
    int error_count = 0;
    
    // 存储原始值（用于转换前）
    struct {
        float finger_clamp;
        float finger_init;
        float finger_flip_wait;
        float finger_flip;
        float finger_max;
        float offset_finger1_zero;
        float offset_finger3_zero;
        float speed_factor;
        float v_flip;
        float a_flip;
        float v_twist;
        float a_twist;
        float v_no_load;
        float a_no_load;
        float v_finger;
        float a_finger;
        float v_no_load_20_70_deg;
        float finger_no_load_start_arm;
        int arm4_zero;
        int arm2_zero;
        int max_current;
        int clamp_current;
        float arm_motion_time_out;
    } raw_values = {0};

    while (fgets(line, sizeof(line), file)) {
        line_num++;
        // 跳过注释行和空行
        if (line[0] == '#' || line[0] == '\n') continue;
        
        char key[64], value[64];
        if (sscanf(line, "%63[^=]= %63[^\n]", key, value) == 2) {
            // 去除key中的尾部空格
            char *end = key + strlen(key) - 1;
            while (end > key && isspace((int)*end)) end--;
            *(end + 1) = '\0';
            // 解析并标记找到的配置项
            if (strcmp(key, "ARM4_ZERO") == 0) {
                raw_values.arm4_zero = atoi(value);
                flags.arm4_zero = true;
            }
            else if (strcmp(key, "ARM2_ZERO") == 0) {
                raw_values.arm2_zero = atoi(value);
                flags.arm2_zero = true;
            }
            else if (strcmp(key, "OFFSET_FINGER_1_ZERO") == 0) {
                raw_values.offset_finger1_zero = atof(value);
                flags.offset_finger1_zero = true;
            }
            else if (strcmp(key, "OFFSET_FINGER_3_ZERO") == 0) {
                raw_values.offset_finger3_zero = atof(value);
                flags.offset_finger3_zero = true;
            }
            else if (strcmp(key, "FINGER_CLAMP") == 0) {
                raw_values.finger_clamp = atof(value);
                flags.finger_clamp = true;
            }
            else if (strcmp(key, "FINGER_INIT") == 0) {
                raw_values.finger_init = atof(value);
                flags.finger_init = true;
            }
            else if (strcmp(key, "FINGER_FLIP_WAIT") == 0) {
                raw_values.finger_flip_wait = atof(value);
                flags.finger_flip_wait = true;
            }
            else if (strcmp(key, "FINGER_FLIP") == 0) {
                raw_values.finger_flip = atof(value);
                flags.finger_flip = true;
            }
            else if (strcmp(key, "FINGER_MAX") == 0) {
                raw_values.finger_max = atof(value);
                flags.finger_max = true;
            }
            else if (strcmp(key, "MAX_CURRENT") == 0) {
                raw_values.max_current = atoi(value);
                flags.max_current = true;
            }
            else if (strcmp(key, "CLAMP_CURRENT") == 0) {
                raw_values.clamp_current = atoi(value);
                flags.clamp_current = true;
            }
            else if (strcmp(key, "SPEED_FACTOR") == 0) {
                raw_values.speed_factor = atof(value);
                flags.speed_factor = true;
            }
            else if (strcmp(key, "V_FLIP") == 0) {
                raw_values.v_flip = atof(value);
                flags.v_flip = true;
            }
            else if (strcmp(key, "A_FLIP") == 0) {
                raw_values.a_flip = atof(value);
                flags.a_flip = true;
            }
            else if (strcmp(key, "V_TWIST") == 0) {
                raw_values.v_twist = atof(value);
                flags.v_twist = true;
            }
            else if (strcmp(key, "A_TWIST") == 0) {
                raw_values.a_twist = atof(value);
                flags.a_twist = true;
            }
            else if (strcmp(key, "V_NO_LOAD") == 0) {
                raw_values.v_no_load = atof(value);
                flags.v_no_load = true;
            }
            else if (strcmp(key, "A_NO_LOAD") == 0) {
                raw_values.a_no_load = atof(value);
                flags.a_no_load = true;
            }
            else if (strcmp(key, "V_FINGER") == 0) {
                raw_values.v_finger = atof(value);
                flags.v_finger = true;
            }
            else if (strcmp(key, "A_FINGER") == 0) {
                raw_values.a_finger = atof(value);
                flags.a_finger = true;
            }
            else if (strcmp(key, "V_NO_LOAD_20_70_DEG") == 0) {
                raw_values.v_no_load_20_70_deg = atof(value);
                flags.v_no_load_20_70_deg = true;
            }
            else if (strcmp(key, "FINGER_NO_LOAD_START_ARM") == 0) {
                raw_values.finger_no_load_start_arm = atof(value);
                flags.finger_no_load_start_arm = true;
            }
            else if (strcmp(key, "ARM_MOTION_TIME_OUT") == 0) {
                raw_values.arm_motion_time_out = atof(value);
                flags.arm_motion_time_out = true;
            }
            else {
                ESP_LOGW(TAG, "未知配置项 '%s' (行 %d)", key, line_num);
            }
        }
        // 忽略其他的情况，例如完全由空格组成的行
        // else {
        //     ESP_LOGW(TAG, "无效配置行: %s (行 %d)", line, line_num);
        //     error_count++;
        // }
    }
    fclose(file);
    // 检查所有必需的配置项
    #define CHECK_FLAG(flag, name) \
        if (!flag) { \
            ESP_LOGE(TAG, "缺少必需的配置项: %s", name); \
            error_count++; \
        }
    
    CHECK_FLAG(flags.arm4_zero, "ARM4_ZERO");
    CHECK_FLAG(flags.arm2_zero, "ARM2_ZERO");
    CHECK_FLAG(flags.offset_finger1_zero, "OFFSET_FINGER_1_ZERO");
    CHECK_FLAG(flags.offset_finger3_zero, "OFFSET_FINGER_3_ZERO");
    CHECK_FLAG(flags.finger_clamp, "FINGER_CLAMP");
    CHECK_FLAG(flags.finger_init, "FINGER_INIT");
    CHECK_FLAG(flags.finger_flip_wait, "FINGER_FLIP_WAIT");
    CHECK_FLAG(flags.finger_flip, "FINGER_FLIP");
    CHECK_FLAG(flags.finger_max, "FINGER_MAX");
    CHECK_FLAG(flags.max_current, "MAX_CURRENT");
    CHECK_FLAG(flags.clamp_current, "CLAMP_CURRENT");
    CHECK_FLAG(flags.speed_factor, "SPEED_FACTOR");
    CHECK_FLAG(flags.v_flip, "V_FLIP");
    CHECK_FLAG(flags.a_flip, "A_FLIP");
    CHECK_FLAG(flags.v_twist, "V_TWIST");
    CHECK_FLAG(flags.a_twist, "A_TWIST");
    CHECK_FLAG(flags.v_no_load, "V_NO_LOAD");
    CHECK_FLAG(flags.a_no_load, "A_NO_LOAD");
    CHECK_FLAG(flags.v_finger, "V_FINGER");
    CHECK_FLAG(flags.a_finger, "A_FINGER");
    CHECK_FLAG(flags.v_no_load_20_70_deg, "V_NO_LOAD_20_70_DEG");
    CHECK_FLAG(flags.finger_no_load_start_arm, "FINGER_NO_LOAD_START_ARM");
    CHECK_FLAG(flags.arm_motion_time_out, "ARM_MOTION_TIME_OUT");
    
    #undef CHECK_FLAG
    
    if (error_count > 0) {
        ESP_LOGE(TAG, "配置文件 %s 有 %d 个错误", path, error_count);
        return -1;
    }
    
    // 所有配置项都存在，进行转换
    const float FINGER_FACTOR = 16384.0f / (2 * 31.416f); // 手指电机每转一圈，手指移动距离31.4mm，两手指间距增加2 * 31.4mm
    #define FINGER_DIST2ENC(x) roundf((x - 52.0f) * FINGER_FACTOR)
    mcfg.finger_clamp  = FINGER_DIST2ENC(raw_values.finger_clamp);
    mcfg.finger_init   = FINGER_DIST2ENC(raw_values.finger_init);
    mcfg.finger_flip_wait = FINGER_DIST2ENC(raw_values.finger_flip_wait);
    mcfg.finger_flip   = FINGER_DIST2ENC(raw_values.finger_flip);
    mcfg.finger_max    = FINGER_DIST2ENC(raw_values.finger_max);
    
    // 计算偏移量
    mcfg.offset_finger1_zero = roundf(raw_values.offset_finger1_zero * FINGER_FACTOR);
    mcfg.offset_finger3_zero = roundf(raw_values.offset_finger3_zero * FINGER_FACTOR);
    
    // 设置电流参数
    mcfg.max_current   = raw_values.max_current;
    mcfg.clamp_current = raw_values.clamp_current;
    
    // 设置速度因子
    const float speed_factor2 = raw_values.speed_factor * raw_values.speed_factor;
    
    // 计算速度/加速度参数
    mcfg.v_flip  = roundf(raw_values.v_flip * raw_values.speed_factor);
    mcfg.a_flip  = roundf(raw_values.a_flip * speed_factor2);
    mcfg.v_twist = roundf(raw_values.v_twist * raw_values.speed_factor);
    mcfg.a_twist = roundf(raw_values.a_twist * speed_factor2);
    mcfg.v_no_load = roundf(raw_values.v_no_load * raw_values.speed_factor);
    mcfg.a_no_load = roundf(raw_values.a_no_load * speed_factor2);
    mcfg.v_finger = roundf(raw_values.v_finger * raw_values.speed_factor);
    mcfg.a_finger = roundf(raw_values.a_finger * speed_factor2);
    
    // 计算联动参数
    mcfg.v_no_load_20_70_deg = roundf(raw_values.v_no_load_20_70_deg * raw_values.speed_factor);
    mcfg.finger_no_load_start_arm = FINGER_DIST2ENC(raw_values.finger_no_load_start_arm);
    
    // 设置零点位置
    mcfg.arm4_zero = raw_values.arm4_zero;
    mcfg.arm2_zero = raw_values.arm2_zero;

    // 设置超时参数
    mcfg.arm_motion_time_out = roundf(1000000 * raw_values.arm_motion_time_out);
    return 0;
}

// 最大电机数量，用于分配内存
const int MAX_MOTOR_COUNT = 4;

// TRAP指令超时（只有通信断开时才会触发）
const int64_t ARM_TRAP_TIME_OUT   = 5000000;

// 方向定义
const bool LEFT = true;
const bool RIGHT = false;
const bool CW = true;
const bool CCW = false;

// 定义梯形运动参数结构体
typedef struct {
    int32_t x1;        // 目标位置
    int16_t v1;        // 终止速度
    int16_t vmax;      // 最大速度
    int16_t a;         // 加速度
    uint8_t current;   // 电流百分比
} TrapParam;

// 运动控制结构体
typedef struct {
    bool init;// = true表示已经初始化
    int32_t finger_zero[2]; // [0]: finger1, [1]: finger3
    int32_t arm_zero[2];    // [0]: arm2, [1]: arm4
    int32_t finger_offset[2]; // [0]: finger1, [1]: finger3
    int32_t arm_offset[2];    // [0]: arm2, [1]: arm4
} MotionCtrl;

// 运动控制结构体，存储电机零点和当前位置信息
MotionCtrl mc = {0};

// 当前电机角度，用于web界面显示
int32_t g_angles[4] = {0};

// 通信接口配置
#define RS485_TX_BUF_SIZE 129
#define RS485_RX_BUF_SIZE 129
#define RS485_BAUD_RATE   1000000 // 1Mbps
#define RS485_UART_PORT   2  // UART0默认是调试串口，UART1和UART2可用
#define RS485_READ_TICS   (50 / portTICK_PERIOD_MS)
// Timeout threshold for UART = number of symbols (~10 tics) with unchanged state on receive pin
#define RS485_READ_TOUT   (3) // 3.5T * 8 = 28 ticks, TOUT=3 -> ~24..33 ticks

void init_rs485(void)
{
    const int uart_num = RS485_UART_PORT;
    const uart_config_t uart_config = {
        .baud_rate = RS485_BAUD_RATE,
        .data_bits = UART_DATA_8_BITS,
        .parity = UART_PARITY_DISABLE,
        .stop_bits = UART_STOP_BITS_1,
        .flow_ctrl = UART_HW_FLOWCTRL_DISABLE,
        .rx_flow_ctrl_thresh = 122,
        .source_clk = UART_SCLK_DEFAULT,
    };
    ESP_ERROR_CHECK(uart_driver_install(uart_num, RS485_TX_BUF_SIZE, RS485_RX_BUF_SIZE, 0, NULL, 0));
    // Configure UART parameters
    ESP_ERROR_CHECK(uart_param_config(uart_num, &uart_config));
    // Set UART pins as per KConfig settings
    ESP_ERROR_CHECK(uart_set_pin(uart_num, RS485_TXD, RS485_RXD, RS485_EN, -1));
    // 启用RX引脚内部上拉电阻（RS485接口芯片发送期间，RX是高阻状态）
    gpio_set_pull_mode(RS485_RXD, GPIO_PULLUP_ONLY);
    // Set RS485 half duplex mode
    ESP_ERROR_CHECK(uart_set_mode(uart_num, UART_MODE_RS485_HALF_DUPLEX));
    // Set read timeout of UART TOUT feature
    ESP_ERROR_CHECK(uart_set_rx_timeout(uart_num, RS485_READ_TOUT));
}

// CRC8计算
static uint8_t crc8(const uint8_t *datagram, int datagramLength)
{
    int i, j;
    uint8_t currentByte;
    uint8_t crc = 0;
    for (i = 0; i < datagramLength; i++)
    {                              // Execute for all bytes of a message
        currentByte = datagram[i]; // Retrieve a byte to be sent from Array
        for (j = 0; j < 8; j++)
        {
            if ((crc >> 7) ^ (currentByte & 0x01)) // update CRC based result of XOR operation
            {
                crc = (crc << 1) ^ 0x07;
            }
            else
            {
                crc = (crc << 1);
            }
            currentByte = currentByte >> 1;
        } // for CRC bit
    }
    return crc;
}

/* 构造主机到电机控制器的数据帧
:param motor_count: 电机数量（1-8）
:param ids: 每个电机的ID列表，长度等于motor_count
:param command_types: 每个电机的指令类型列表
:param data_list: 每个电机的数据列表，元素为bytes类型
:return: 完整的字节数据帧
*/
static int build_command_frame(uint8_t motor_count, const uint8_t *ids, 
                        const uint8_t *command_types, 
                        const uint8_t **data_list, const int *data_lengths,
                        uint8_t *output, int output_size) {
    if (motor_count < 1 || motor_count > 8) {
        ESP_LOGE(TAG, "电机数量必须在1-8之间");
        return -1;
    }
    
    // 计算指令块总长度
    int instruction_length = 0;
    for (int i = 0; i < motor_count; i++) {
        instruction_length += 1 + 1 + data_lengths[i]; // ID + 命令类型 + 数据
    }
    
    // 计算总长度: 2(FF FF) + 1(长度) + 1(电机数量) + 指令块长度 + 1(CRC)
    int total_length = 2 + 1 + 1 + instruction_length + 1;
    if (total_length > output_size) {
        ESP_LOGE(TAG, "输出缓冲区太小");
        return -1;
    }
    // 构建帧
    uint8_t *ptr = output;
    *ptr++ = 0xFF;
    *ptr++ = 0xFF;
    *ptr++ = (uint8_t)total_length;
    *ptr++ = motor_count;
    
    // 添加指令块
    for (int i = 0; i < motor_count; i++) {
        *ptr++ = ids[i];
        *ptr++ = command_types[i];
        memcpy(ptr, data_list[i], data_lengths[i]);
        ptr += data_lengths[i];
    }
    
    // 计算并添加CRC
    uint8_t crc = crc8(output, ptr - output);
    *ptr++ = crc;
    
    return (int)(ptr - output);
}

// 接收响应
static int receive_response(uint8_t *buffer, int expected_length) {
    int state = 0;  // 0: 等待帧头第一字节，1: 等待帧头第二字节，2: 接收数据体
    int count = 0;
    while (true) {
        uint8_t byte;
        int n = uart_read_bytes(RS485_UART_PORT, &byte, 1, RS485_READ_TICS);// 最长50ms等待
        if (n != 1) {
            ESP_LOGE(TAG, "读取串口失败");
            return -1;
        }

        // 状态机处理
        switch (state) {
            case 0:  // 等待帧头0xFF
                if (byte == 0xFF) {
                    buffer[0] = byte;
                    state = 1;
                }
                break;
                
            case 1:  // 等待第二个0xFF
                if (byte == 0xFF) {
                    buffer[1] = byte;
                    state = 2;
                    count = 2;
                } else {
                    // 非连续0xFF，重置状态
                    state = (byte == 0xFF) ? 1 : 0;
                }
                break;
                
            case 2:  // 接收数据体
                buffer[count] = byte;
                if (++count >= expected_length) {
                    return count;
                }
                break;
        }
    }
    return 0;
}

// 解析状态响应
static int parse_stat_response(const uint8_t *frame, size_t length, 
                        uint16_t *cmd_count, uint8_t *trap_status, 
                        int8_t *temperature, int32_t *pos, int16_t *voltage) {
    if (length != 15) {
        ESP_LOGE(TAG, "状态响应长度应为15字节");
        return -1;
    }
    
    // 检查CRC
    uint8_t calculated_crc = crc8(frame, 14);
    if (calculated_crc != frame[14]) {
        ESP_LOGE(TAG, "CRC校验失败: 计算值%02X, 接收值%02X", calculated_crc, frame[14]);
        return -1;
    }
    
    // 检查标志位
    if (frame[3] != 0) {
        ESP_LOGE(TAG, "从机数据标志位不为零: %d", frame[3]);
        return -1;
    }
    
    // 解析字段
    *cmd_count = (frame[5] << 8) | frame[4];
    *trap_status = frame[6];
    *temperature = (int8_t)frame[7];
    *pos = (frame[11] << 24) | (frame[10] << 16) | (frame[9] << 8) | frame[8];
    *voltage = (frame[13] << 8) | frame[12];
    
    return 0;
}
static int parse_other_response(const uint8_t *frame, size_t length) {
    if (length != 5) {
        ESP_LOGE(TAG, "状态响应长度应为5字节");
        return -1;
    }
    
    // 检查CRC
    uint8_t calculated_crc = crc8(frame, 4);
    if (calculated_crc != frame[4]) {
        ESP_LOGE(TAG, "CRC校验失败: 计算值%02X, 接收值%02X", calculated_crc, frame[4]);
        return -1;
    }
    
    // 检查标志位
    if (frame[3] != 0) {
        ESP_LOGE(TAG, "从机数据标志位不为零: %d", frame[3]);
        return -1;
    }
    
    return 0;
}
// 发送使能命令(最多MAX_MOTOR_COUNT个电机)
// ff ff 11 04 01 01 00 02 01 00 03 01 00 04 01 00 c9
static int cmd_enable(const uint8_t *id_list, int count, uint8_t enable) {
    uint8_t enable_data = enable;
    const uint8_t *data_list[MAX_MOTOR_COUNT];
    int data_lengths[MAX_MOTOR_COUNT];
    uint8_t command_types[MAX_MOTOR_COUNT];
    
    for (int i = 0; i < count; i++) {
        data_list[i] = &enable_data;
        data_lengths[i] = 1;
        command_types[i] = 0x01; // 使能命令
    }
    
    uint8_t frame[5 + 3*MAX_MOTOR_COUNT];
    int frame_length = build_command_frame((uint8_t)count, id_list, command_types, 
                                         (const uint8_t **)data_list, data_lengths, 
                                         frame, sizeof(frame));
    
    if (frame_length < 0) {
        return -1;
    }

    // 发送数据
    int bytes_written = uart_write_bytes(RS485_UART_PORT, frame, frame_length);
    if (bytes_written != frame_length) {
        ESP_LOGE(TAG, "发送使能命令失败，写入 %d/%d 字节", bytes_written, frame_length);
    }
    
    // 接收响应
    uint8_t response[5];
    int response_length = receive_response(response, sizeof(response));
    if (response_length < 5) {
        ESP_LOGE(TAG, "接收响应失败");
        return -1;
    }
    
    // 检查响应
    if (parse_other_response(response, response_length) != 0) {
        ESP_LOGE(TAG, "响应标志位错误: %d", response[3]);
        return -1;
    }
    
    return 0;
}

// 查询状态
static int cmd_stat(uint8_t id, uint16_t *cmd_count, uint8_t *trap_status, 
             int8_t *temperature, int32_t *pos, int16_t *voltage) {
    uint8_t ids[] = {id};
    uint8_t command_types[] = {0x00}; // 查询命令
    const uint8_t *data_list[] = {NULL};
    int data_lengths[] = {0};
    
    uint8_t frame[5 + 3*MAX_MOTOR_COUNT];
    int frame_length = build_command_frame(1, ids, command_types, 
                                         data_list, data_lengths, 
                                         frame, sizeof(frame));
    if (frame_length < 0) {
        return -1;
    }
    
    // 发送数据
    int bytes_written = uart_write_bytes(RS485_UART_PORT, frame, frame_length);
    if (bytes_written != frame_length) {
        ESP_LOGE(TAG, "发送查询命令失败，写入 %d/%d 字节", bytes_written, frame_length);
        return -1;
    }
    
    // 接收响应
    uint8_t response[15];
    int response_length = receive_response(response, sizeof(response));
    if (response_length < 15) {
        ESP_LOGE(TAG, "接收响应失败");
        return -1;
    }
    int ret = parse_stat_response(response, response_length, cmd_count, trap_status, temperature, pos, voltage);
    // 更新电机角度信息，用于web界面显示
    if(id <= 4){
        g_angles[id - 1] = *pos;
    }
    return ret;
}

// 获取当前位置
static int32_t cmd_get_pos(uint8_t id) {
    uint16_t cmd_count;
    uint8_t trap_status;
    int8_t temperature;
    int32_t pos;
    int16_t voltage;
    
    if (cmd_stat(id, &cmd_count, &trap_status, &temperature, &pos, &voltage) == 0) {
        // ESP_LOGI(TAG, "cmd_count=%d trap_status=%d temperature=%d pos=%d voltage=%d\n",
        //     cmd_count, trap_status, temperature, pos, voltage);
        return pos;
    }
    return 0;
}

// 等待运动完成(不检查位置信息)
int32_t cmd_wait_motion(uint8_t id) {
    // ESP_LOGI(TAG, "等待%d号控制板完成运动控制", id);
    int64_t start_time = esp_timer_get_time();
    int64_t current_time = start_time;
    uint8_t trap_status;
    
    do {
        vTaskDelay(1);// 最小延时确保其他任务执行
        uint16_t cmd_count;
        int8_t temperature;
        int32_t pos;
        int16_t voltage;
        
        if (cmd_stat(id, &cmd_count, &trap_status, &temperature, &pos, &voltage) != 0) {
            return 0;
        }
        current_time = esp_timer_get_time();
        if (trap_status == 0) {
            // ESP_LOGI(TAG, "耗时: %.2fms", (current_time - start_time)/1000.0f);
            return pos;
        }

    } while (current_time - start_time < ARM_TRAP_TIME_OUT);
    
    ESP_LOGE(TAG, "等待运动超时");
    return 0;
}

// 发送梯形运动命令
static int cmd_trap(const uint8_t *id_list, int count, bool zero, const TrapParam *trap_list) 
{
    // 准备数据
    const uint8_t *data_list[MAX_MOTOR_COUNT];
    int data_lengths[MAX_MOTOR_COUNT];
    uint8_t command_types[MAX_MOTOR_COUNT];
    uint8_t motor_data[MAX_MOTOR_COUNT][11]; // 每个电机的数据缓冲区
    
    for (int i = 0; i < count; i++) {
        // 打包数据: x1(4), v1(2), vmax(2), a(2), current(1)
        uint8_t *ptr = motor_data[i];
        *ptr++ = (trap_list[i].x1) & 0xFF;
        *ptr++ = (trap_list[i].x1 >> 8) & 0xFF;
        *ptr++ = (trap_list[i].x1 >> 16) & 0xFF;
        *ptr++ = (trap_list[i].x1 >> 24) & 0xFF;
        
        *ptr++ = trap_list[i].v1 & 0xFF;
        *ptr++ = (trap_list[i].v1 >> 8) & 0xFF;
        
        *ptr++ = trap_list[i].vmax & 0xFF;
        *ptr++ = (trap_list[i].vmax >> 8) & 0xFF;
        
        *ptr++ = trap_list[i].a & 0xFF;
        *ptr++ = (trap_list[i].a >> 8) & 0xFF;
        
        *ptr++ = trap_list[i].current;
        
        data_list[i] = motor_data[i];
        data_lengths[i] = 11;
        command_types[i] = zero ? 0x03 : 0x02; // 回零或绝对位置指令
    }
    
    // 构建帧
    uint8_t frame[5 + 3*MAX_MOTOR_COUNT + MAX_MOTOR_COUNT*11];
    int frame_length = build_command_frame((uint8_t)count, id_list, command_types, 
                                          data_list, data_lengths, 
                                          frame, sizeof(frame));
    if (frame_length < 0) {
        return -1;
    }
    
    // 发送数据
    int bytes_written = uart_write_bytes(RS485_UART_PORT, frame, frame_length);
    if (bytes_written != frame_length) {
        ESP_LOGE(TAG, "发送梯形命令失败，写入 %d/%d 字节", bytes_written, frame_length);
        return -1;
    }
    
    // 接收响应
    uint8_t response[5];
    int response_length = receive_response(response, sizeof(response));
    if (response_length < 5) {
        ESP_LOGE(TAG, "接收梯形响应失败");
        return -1;
    }
    
    return parse_other_response(response, response_length);
}

// 计算旋转臂最短路径目标位置函数
static int32_t calculate_arm_target(int32_t current_pos, int32_t target_zero, int32_t full_steps)
{
    // 1. 映射当前位置到单圈范围 [0, full_steps)
    int32_t current_normalized = current_pos % full_steps;
    if (current_normalized < 0) current_normalized += full_steps;
    
    // 2. 映射目标零点到单圈范围
    int32_t target_normalized = target_zero % full_steps;
    if (target_normalized < 0) target_normalized += full_steps;
    
    // 3. 计算两个可能方向的差值
    int32_t diff_forward = target_normalized - current_normalized;
    int32_t diff_backward = (target_normalized > current_normalized) ? 
        (target_normalized - full_steps - current_normalized) : 
        (full_steps - current_normalized + target_normalized);
    
    // 4. 选择最短路径
    int32_t min_diff = (abs(diff_forward) < abs(diff_backward)) ? 
        diff_forward : diff_backward;
    
    // 5. 计算最终目标位置
    return current_pos + min_diff;
}
int cmd_zero(void) 
{
    // 配置参数
    const uint8_t current_low = 15; // 电流15%
    const uint8_t current_high = 100; // 电流100%
    const int16_t accel = 20;
    const int16_t speed_slow = 30;
    const int16_t speed_fast = 100;
    
    // 禁用全部电机
    if (cmd_enable((uint8_t[]){1, 2, 3, 4}, 4, 0) != 0) {
        ESP_LOGE(TAG, "禁用电机失败");
        return -1;
    }
    
    // ----------------- 手指1回零 -----------------
    // 同时使能手指1和旋转臂2
    if (cmd_enable((uint8_t[]){1, 2}, 2, 1) != 0) {
        ESP_LOGE(TAG, "使能手指1和旋转臂2失败");
        return -1;
    }
    
    int32_t finger1_pos_old = cmd_get_pos(1);    
    // 创建运动参数 - 回零运动
    TrapParam finger1_zero_param = {
        .x1 = finger1_pos_old - 10000,
        .v1 = 0,
        .vmax = speed_slow,
        .a = accel,
        .current = current_low
    };
    
    // 发送梯形运动命令 - 只控制手指1回零
    if (cmd_trap((uint8_t[]){1}, 1, true, &finger1_zero_param) != 0) {
        ESP_LOGE(TAG, "手指1回零运动失败");
        return -1;
    }
    
    int32_t current_finger1_pos = cmd_wait_motion(1);
    int32_t motion_distance = current_finger1_pos - finger1_pos_old;
    ESP_LOGI(TAG, "手指1回零移动距离: %ld, 当前位置: %ld", motion_distance, current_finger1_pos);
    
    if (abs(motion_distance) > 9500) {
        cmd_enable((uint8_t[]){1, 2, 3, 4}, 4, 0);
        ESP_LOGE(TAG, "手指1移动距离过长，回零失败");
        return -1;
    }
    
    // 补偿偏移
    current_finger1_pos += mcfg.offset_finger1_zero;
    TrapParam finger1_adj_param = {
        .x1 = current_finger1_pos,
        .v1 = 0,
        .vmax = speed_slow,
        .a = accel,
        .current = current_high
    };
    
    // 只控制手指1调整位置
    if (cmd_trap((uint8_t[]){1}, 1, false, &finger1_adj_param) != 0) {
        ESP_LOGE(TAG, "手指1位置调整失败");
        return -1;
    }
    cmd_wait_motion(1);
    
    // 获取旋转臂2当前位置
    int32_t arm2_old = cmd_get_pos(2);
    
    // 计算旋转臂2目标位置（考虑多圈）
    int32_t arm2_zero_pos = calculate_arm_target(arm2_old, mcfg.arm2_zero, 16384);
    
    // 计算手指1补偿量
    int32_t finger1_target = current_finger1_pos + (arm2_zero_pos - arm2_old) / 2;
    
    // 准备多轴运动 - 同时控制手指1和旋转臂2
    TrapParam multi_params[2] = {
        { // 手指1
            .x1 = finger1_target,
            .v1 = 0,
            .vmax = speed_fast,
            .a = accel,
            .current = current_high
        },
        { // 旋转臂2
            .x1 = arm2_zero_pos,
            .v1 = 0,
            .vmax = speed_fast * 2,  // 两倍速度
            .a = accel * 2,          // 两倍加速度
            .current = current_high
        }
    };
    
    // 同时控制两个电机, zero参数改为true，碰到障碍回立即停止
    if (cmd_trap((uint8_t[]){1, 2}, 2, true, multi_params) != 0) {
        ESP_LOGE(TAG, "多轴运动失败");
        return -1;
    }
    cmd_wait_motion(2);  // 等待两个电机都完成

    // 检查旋转臂角度是否正确
    vTaskDelay(pdMS_TO_TICKS(20));
    const int MAX_ERROR = round(8192 * (2.0/90.0)); // 最多允许2°误差
    int32_t arm2_real_pos = cmd_get_pos(2);
    int32_t error2 = abs(arm2_real_pos - arm2_zero_pos);
    if(error2 > MAX_ERROR){
        cmd_enable((uint8_t[]){1, 2, 3, 4}, 4, 0);
        ESP_LOGE(TAG, "手臂电机堵转故障");
        return -1;
    }

    // ----------------- 手指3回零 -----------------
    // 同时使能手指3和旋转臂4
    if (cmd_enable((uint8_t[]){3, 4}, 2, 1) != 0) {
        ESP_LOGE(TAG, "使能手指3和旋转臂4失败");
        return -1;
    }
    
    int32_t finger3_pos_old = cmd_get_pos(3);
    // 只控制手指3回零
    TrapParam finger3_zero_param = {
        .x1 = finger3_pos_old - 10000,
        .v1 = 0,
        .vmax = speed_slow,
        .a = accel,
        .current = current_low
    };
    
    if (cmd_trap((uint8_t[]){3}, 1, true, &finger3_zero_param) != 0) {
        ESP_LOGE(TAG, "手指3回零失败");
        return -1;
    }
    
    int32_t current_finger3_pos = cmd_wait_motion(3);
    motion_distance = current_finger3_pos - finger3_pos_old;
    ESP_LOGI(TAG, "手指3回零移动距离: %ld, 当前位置: %ld", motion_distance, current_finger3_pos);
    
    if (abs(motion_distance) > 9500) {
        cmd_enable((uint8_t[]){1, 2, 3, 4}, 4, 0);
        ESP_LOGE(TAG, "手指3移动距离过长，回零失败");
        return -1;
    }
    
    // 补偿偏移
    current_finger3_pos += mcfg.offset_finger3_zero;
    TrapParam finger3_adj_param = {
        .x1 = current_finger3_pos,
        .v1 = 0,
        .vmax = speed_slow,
        .a = accel,
        .current = current_high
    };
    
    // 只控制手指3调整位置
    if (cmd_trap((uint8_t[]){3}, 1, false, &finger3_adj_param) != 0) {
        ESP_LOGE(TAG, "手指3位置调整失败");
        return -1;
    }
    cmd_wait_motion(3);
    
    // 获取旋转臂4当前位置
    int32_t arm4_old = cmd_get_pos(4);
    
    // 计算旋转臂4目标位置（考虑多圈）
    int32_t arm4_zero_pos = calculate_arm_target(arm4_old, mcfg.arm4_zero, 16384);
    
    // 计算手指3补偿量
    int32_t finger3_target = current_finger3_pos + (arm4_zero_pos - arm4_old) / 2;
    
    // 准备多轴运动 - 同时控制手指3和旋转臂4
    TrapParam multi_params2[2] = {
        { // 手指3
            .x1 = finger3_target,
            .v1 = 0,
            .vmax = speed_fast,
            .a = accel,
            .current = current_high
        },
        { // 旋转臂4
            .x1 = arm4_zero_pos,
            .v1 = 0,
            .vmax = speed_fast * 2,  // 两倍速度
            .a = accel * 2,          // 两倍加速度
            .current = current_high
        }
    };
    
    // 同时控制两个电机, zero参数改为true，碰到障碍回立即停止
    if (cmd_trap((uint8_t[]){3, 4}, 2, true, multi_params2) != 0) {
        ESP_LOGE(TAG, "多轴运动失败");
        return -1;
    }
    cmd_wait_motion(4);  // 等待两个电机都完成
    
    // 检查旋转臂角度是否正确
    vTaskDelay(pdMS_TO_TICKS(20));
    int32_t arm4_real_pos = cmd_get_pos(4);
    int32_t error4 = abs(arm4_real_pos - arm4_zero_pos);
    if(error4 > MAX_ERROR){
        cmd_enable((uint8_t[]){1, 2, 3, 4}, 4, 0);
        ESP_LOGE(TAG, "手臂电机堵转故障");
        return -1;
    }
    
    ESP_LOGI(TAG, "回零点成功:");
    ESP_LOGI(TAG, "  手指1位置: %ld", finger1_target);
    ESP_LOGI(TAG, "  旋转臂2位置: %ld", arm2_zero_pos);
    ESP_LOGI(TAG, "  手指3位置: %ld", finger3_target);
    ESP_LOGI(TAG, "  旋转臂4位置: %ld", arm4_zero_pos);

    // 初始化运动控制
    mc.finger_zero[0]   = finger1_target;
    mc.arm_zero[0]      = arm2_zero_pos;
    mc.finger_zero[1]   = finger3_target;
    mc.arm_zero[1]      = arm4_zero_pos;
    mc.finger_offset[0] = 0;
    mc.finger_offset[1] = 0;
    mc.arm_offset[0]    = 0;
    mc.arm_offset[1]    = 0;
    mc.init = true;
    return 0;
}

// 移动两个手指
static int move_two_finger_raw(int32_t target, uint8_t current) {
    if (!mc.init) {
        ESP_LOGE(TAG, "需要先调用cmd_zero");
        return -1;
    }
    if(mc.finger_offset[0] == target && mc.finger_offset[1] == target ){
        // 已经在指定位置了，无需处理
        ESP_LOGI(TAG, "两个手指已经处于指定位置");
        return 0;
    }
    mc.finger_offset[0] = target;
    mc.finger_offset[1] = target;
    
    int32_t finger1 = mc.finger_zero[0] + mc.arm_offset[0] / 2 + mc.finger_offset[0];
    int32_t finger3 = mc.finger_zero[1] + mc.arm_offset[1] / 2 + mc.finger_offset[1];
    
    // 准备多轴运动 - 同时控制手指1和手指3
    TrapParam multi_params2[2] = {
        { // 手指1
            .x1 = finger1,
            .v1 = 0,
            .vmax = mcfg.v_finger,
            .a = mcfg.a_finger,
            .current = current
        },
        { // 手指3
            .x1 = finger3,
            .v1 = 0,
            .vmax = mcfg.v_finger,
            .a = mcfg.a_finger,
            .current = current
        }
    };
    
    // 同时控制两个电机
    if (cmd_trap((uint8_t[]){1, 3}, 2, false, multi_params2) != 0) {
        return -1;
    }
    cmd_wait_motion(3);
    return 0;
}

// 手指初始化位置
int two_finger_init(void) {
    return move_two_finger_raw(mcfg.finger_init, mcfg.max_current);
}

// 手指夹紧
int two_finger_clamp(void) {
    return move_two_finger_raw(mcfg.finger_clamp, mcfg.clamp_current);
}

// 检查手臂位置是否在允许误差范围内
static int check_arm_pos(uint8_t id, int32_t real, int32_t expect) {
    int64_t start_time = esp_timer_get_time();
    const int MAX_ERROR = round(8192 * (2.0/90.0)); // 最多允许2°误差
    int32_t error = abs(real - expect);
    // ESP_LOGI(TAG, "手臂角度误差 %ld", error);
    
    if (error > MAX_ERROR) {
        // ESP_LOGI(TAG, "手臂角度超差，等待误差合格。当前误差%ld，最大允许%d", error, MAX_ERROR);
        while (true) {
            vTaskDelay(1);// 最小延时确保其他任务执行
            int32_t current_pos = cmd_get_pos(id);
            error = abs(current_pos - expect);
            int64_t current_time = esp_timer_get_time();
            
            if (error <= MAX_ERROR) {
                // ESP_LOGI(TAG, "耗时: %.2fms", (current_time - start_time) / 1000.0f);
                break;
            }
            
            if (current_time - start_time > mcfg.arm_motion_time_out) {
                ESP_LOGE(TAG, "运动控制超时，可能是出现了电机堵转问题");
                uint8_t ids[] = {1, 2, 3, 4};
                cmd_enable(ids, 4, 0);
                ESP_LOGE(TAG, "关闭全部电机");
                return -1;
            }
        }
    }
    return 0;
}
// 等待电机位置超过指定点
static int wait_motion_by_pos(uint8_t id, int32_t now, int32_t ret, int32_t target) {
    // ESP_LOGI(TAG, "等待电机 %d 超过指定位置", id);
    // ESP_LOGI(TAG, "%ld --> %ld(在此处返回) --> %ld", now, ret, target);
    int64_t start_time = esp_timer_get_time();
    
    while (true) {
        vTaskDelay(1);// 最小延时确保其他任务执行
        int32_t pos = cmd_get_pos(id);
        int64_t current_time = esp_timer_get_time();
        
        if (current_time - start_time > mcfg.arm_motion_time_out) {
            ESP_LOGE(TAG, "运动控制超时");
            uint8_t ids[] = {1, 2, 3, 4};
            cmd_enable(ids, 4, 0);
            ESP_LOGE(TAG, "关闭全部电机");
            return -1;
        }
        
        if (target > now && pos > ret) break;
        if (target < now && pos < ret) break;
    }
    
    // ESP_LOGI(TAG, "耗时: %.2fms", (esp_timer_get_time() - start_time) / 1000.0f);
    return 0;
}
// 旋转机械臂（带手指联动）
static int move_arm(int angle, bool left, uint8_t finger_current, int speed, int accel, int wait) {
    uint8_t id_list[2];
    int index;
    
    if (left) {
        id_list[0] = 3; // 手指
        id_list[1] = 4; // 手臂
        index = 1;
    } else {
        id_list[0] = 1; // 手指
        id_list[1] = 2; // 手臂
        index = 0;
    }
    
    // 手臂旋转电机目前位置
    int32_t arm_now = mc.arm_zero[index] + mc.arm_offset[index];
    // 计算函数返回时的电机位置
    int32_t arm_ret = mc.arm_zero[index] + mc.arm_offset[index] + 8192 * (wait / 90.0);
    // 更新手臂位置
    mc.arm_offset[index] += 8192 * (angle / 90);
    // 手臂目标位置
    int32_t arm_target = mc.arm_zero[index] + mc.arm_offset[index];
    // 手指目标位置
    int32_t finger_target = mc.finger_zero[index] + mc.arm_offset[index] / 2 + mc.finger_offset[index];

    TrapParam params[2] = {
        { // 手指电机
            .x1 = finger_target,
            .v1 = 0,
            .vmax = speed,
            .a = accel,
            .current = finger_current
        },
        { // 手臂电机
            .x1 = arm_target,
            .v1 = 0,
            .vmax = 2 * speed,
            .a = 2 * accel,
            .current = mcfg.max_current
        }
    };
    
    cmd_trap(id_list, 2, false, params);
    int ret = 0;
    if (wait == 0) {
        int32_t real_pos = cmd_wait_motion(id_list[1]);
        ret = check_arm_pos(id_list[1], real_pos, arm_target);
    } else {
        ret = wait_motion_by_pos(id_list[1], arm_now, arm_ret, arm_target);
    }
    return ret;
}

// 单独移动手臂（不带手指联动）
static int move_arm_without_finger(int angle, bool left, int speed, int accel) {
    uint8_t id;
    int index;
    
    if (left) {
        id = 4;
        index = 1;
    } else {
        id = 2;
        index = 0;
    }
    
    // 更新手臂位置
    mc.arm_offset[index] += 8192 * (angle / 90);
    // 手臂目标位置
    int32_t arm_target = mc.arm_zero[index] + mc.arm_offset[index];
    
    TrapParam param = {
        .x1 = arm_target,
        .v1 = 0,
        .vmax = 2 * speed,
        .a = 2 * accel,
        .current = mcfg.max_current
    };
    
    cmd_trap(&id, 1, false, &param);
    int32_t real_pos = cmd_wait_motion(id);
    return check_arm_pos(id, real_pos, arm_target);
}

// 移动单根手指
static int move_single_finger_raw(bool left, int32_t pos, int speed, int accel, uint8_t current, int wait) {
    uint8_t id;
    int index;
    
    if (left) {
        id = 3;
        index = 1;
    } else {
        id = 1;
        index = 0;
    }
    
    if (mc.finger_offset[index] == pos) {
        ESP_LOGW(TAG, "手指已经处于该位置");
        return 0;
    }
    
    // 手指电机当前位置
    int32_t finger_now = mc.finger_zero[index] + mc.arm_offset[index] / 2 + mc.finger_offset[index];
    // 计算返回时的位置
    int32_t finger_ret = mc.finger_zero[index] + mc.arm_offset[index] / 2 + wait;
    // 更新手指位置
    mc.finger_offset[index] = pos;
    // 手指目标位置
    int32_t finger_target = mc.finger_zero[index] + mc.arm_offset[index] / 2 + mc.finger_offset[index];
    
    TrapParam param = {
        .x1 = finger_target,
        .v1 = 0,
        .vmax = speed,
        .a = accel,
        .current = current
    };
    
    cmd_trap(&id, 1, false, &param);
    
    if (wait == 0) {
        cmd_wait_motion(id);
        return 0;
    } else {
        return wait_motion_by_pos(id, finger_now, finger_ret, finger_target);
    }
}

// 手指锁紧
static void move_finger_lock(bool left) {
    move_single_finger_raw(left, mcfg.finger_clamp, mcfg.v_finger, mcfg.a_finger, mcfg.clamp_current, 0);
}

// 手指初始化位置
static void move_finger_init(bool left) {
    move_single_finger_raw(left, mcfg.finger_init, mcfg.v_finger, mcfg.a_finger, mcfg.max_current, 0);
}

// 手指翻转到指定位置
static int move_finger_flip(bool left, int wait) {
    return move_single_finger_raw(left, mcfg.finger_flip, mcfg.v_finger, mcfg.a_finger, mcfg.max_current, wait);
}

// 手臂空转90度
static int arm_90_no_load(bool left, bool no_finger_return) {
    const int angle = 90;
    move_finger_init(left);
    
    uint8_t id;
    int index;
    
    if (left) {
        id = 3;
        index = 1;
    } else {
        id = 1;
        index = 0;
    }
    
    if (mc.finger_offset[index] != mcfg.finger_init) {
        ESP_LOGE(TAG, "finger_offset 不等于 FINGER_INIT");
    }
    
    // 手指当前位置
    int32_t finger_now = mc.finger_zero[index] + mc.arm_offset[index] / 2 + mcfg.finger_init;
    // 计算返回位置
    int32_t finger_ret = mc.finger_zero[index] + mc.arm_offset[index] / 2 + mcfg.finger_no_load_start_arm;
    // 计算角度对应的位置
    int32_t arm_90_deg = round(8192.0 / 2);
    int32_t arm_20_deg = round(8192.0 * (20.0/90.0) / 2);
    int32_t arm_70_deg = round(8192.0 * (70.0/90.0) / 2);
    
    int32_t finger_target_stage1, finger_target_stage2, finger_target_stage3;
    
    if (!no_finger_return) {
        finger_target_stage1 = mc.finger_zero[index] + mc.arm_offset[index] / 2 + mcfg.finger_max + arm_20_deg;
        finger_target_stage2 = mc.finger_zero[index] + mc.arm_offset[index] / 2 + mcfg.finger_max + arm_70_deg;
        finger_target_stage3 = mc.finger_zero[index] + mc.arm_offset[index] / 2 + mcfg.finger_init + arm_90_deg;
        
        TrapParam param1 = {
            .x1 = finger_target_stage1,
            .v1 = mcfg.v_no_load_20_70_deg / 2,
            .vmax = mcfg.v_finger,
            .a = mcfg.a_finger,
            .current = mcfg.max_current
        };
        cmd_trap(&id, 1, false, &param1);
        
        TrapParam param2 = {
            .x1 = finger_target_stage2,
            .v1 = mcfg.v_no_load_20_70_deg / 2,
            .vmax = mcfg.v_no_load,
            .a = mcfg.a_no_load,
            .current = mcfg.max_current
        };
        cmd_trap(&id, 1, false, &param2);
        
        TrapParam param3 = {
            .x1 = finger_target_stage3,
            .v1 = 0,
            .vmax = mcfg.v_finger,
            .a = mcfg.a_finger,
            .current = mcfg.max_current
        };
        cmd_trap(&id, 1, false, &param3);
    } else {
        finger_target_stage1 = mc.finger_zero[index] + mc.arm_offset[index] / 2 + mcfg.finger_max + arm_20_deg;
        finger_target_stage2 = mc.finger_zero[index] + mc.arm_offset[index] / 2 + mcfg.finger_max + arm_90_deg;
        mc.finger_offset[index] = mcfg.finger_max;
        
        TrapParam param1 = {
            .x1 = finger_target_stage1,
            .v1 = mcfg.v_no_load_20_70_deg / 2,
            .vmax = mcfg.v_finger,
            .a = mcfg.a_finger,
            .current = mcfg.max_current
        };
        cmd_trap(&id, 1, false, &param1);
        
        TrapParam param2 = {
            .x1 = finger_target_stage2,
            .v1 = 0,
            .vmax = mcfg.v_no_load,
            .a = mcfg.a_no_load,
            .current = mcfg.max_current
        };
        cmd_trap(&id, 1, false, &param2);
    }
    
    // 等待手指到达指定位置
    if(wait_motion_by_pos(id, finger_now, finger_ret, finger_target_stage1)){
        return -1;
    }
    // 空转90度
    if(move_arm_without_finger(angle, left, mcfg.v_no_load, mcfg.a_no_load)){
        return -1;
    }
    
    if (!no_finger_return) {
        cmd_wait_motion(id);
        move_finger_lock(left);
    }
    return 0;
}


int motions(const char *actions) 
{
    int ret = 0;
    int64_t start_time_motions = esp_timer_get_time();
    if (!mc.init) {
        ESP_LOGE(TAG, "需要先调用cmd_zero");
        return -1;
    }
    
    const char *p = actions;
    // int i = 0;
    
    while (*p != '\0') {
        int64_t start_time = esp_timer_get_time();
        // i++;
        
        // 跳过空格
        while (*p == ' ') p++;
        if (*p == '\0') break;
        
        // 读取当前指令（两个字符）
        char action[4] = {p[0], p[1], '\0', '\0'};
        p += 2;
        
        // 处理当前指令
        bool left;
        if (action[0] == 'L') {
            left = LEFT;
        } else if (action[0] == 'R') {
            left = RIGHT;
        } else {
            ESP_LOGE(TAG, "未知指令: %s", action);
            return -1;
        }
        
        // 根据指令类型处理
        if (action[1] == '0') {
            move_finger_init(left);
            move_finger_lock(left);
        } 
        else if (action[1] == '1') {
            ret = move_finger_flip(left, mcfg.finger_flip_wait);
        } 
        else if (action[1] == '2') {
            // 跳过空格，预读下一条指令
            while (*p == ' ') p++;
            if (*p == '\0') {
                ESP_LOGE(TAG, "%s不能位于序列末尾", action);
                return -1;
            }
            
            char next_action[3] = {p[0], p[1], '\0'};
            
            // 检查下一条指令是否为"+N"格式
            if (next_action[0] != action[0] || next_action[1] != '+' || p[2] != 'N') {
                ESP_LOGE(TAG, "%s的下一条指令必须为%c+N, 实际为%s", action, action[0], next_action);
                return -1;
            }
            
            // 继续读取+N后的部分
            p += 3; // 跳过下一条指令（三个字符：R+N）
            
            // 跳过空格，尝试读取下下条指令
            while (*p == ' ') p++;
            
            bool close_finger = false;
            if (*p != '\0') {
                // 检查下下条指令是否为"X0"格式
                char next_next_action[3] = {p[0], p[1], '\0'};
                
                // 如果是指令末尾的夹紧操作
                if (next_next_action[0] == action[0] && next_next_action[1] == '0') {
                    close_finger = true;
                    p += 2; // 跳过后面的夹紧指令
                }
            }
            
            ret = arm_90_no_load(left, !close_finger);
        } 
        else if (action[1] == '+' || action[1] == '-' || action[1] == '*') {
            // 检查是否完整动作指令
            action[2] = p[0];
            if (action[2] == 'T' || action[2] == 'F') {
                // 如果动作指令被分割，移动指针
                if (action[1] != '+' && action[1] != '-' && action[1] != '*') {
                    ESP_LOGE(TAG, "无效指令格式: %c%c%c", action[0], action[1], p[0]);
                    return -1;
                }
                p++; // 跳过最后一个字符
                
                const char *op = &action[1];
                if (strncmp(op, "*T", 2) == 0) {
                    ret = move_arm(-180, left, mcfg.clamp_current, mcfg.v_twist, mcfg.a_twist, -170);
                } 
                else if (strncmp(op, "+T", 2) == 0) {
                    ret = move_arm(90, left, mcfg.clamp_current, mcfg.v_twist, mcfg.a_twist, 80);
                } 
                else if (strncmp(op, "-T", 2) == 0) {
                    ret = move_arm(-90, left, mcfg.clamp_current, mcfg.v_twist, mcfg.a_twist, -80);
                } 
                else if (strncmp(op, "*F", 2) == 0) {
                    ret = move_arm(-180, left, mcfg.clamp_current, mcfg.v_flip, mcfg.a_flip, 0);
                } 
                else if (strncmp(op, "+F", 2) == 0) {
                    ret = move_arm(90, left, mcfg.clamp_current, mcfg.v_flip, mcfg.a_flip, 0);
                } 
                else if (strncmp(op, "-F", 2) == 0) {
                    ret = move_arm(-90, left, mcfg.clamp_current, mcfg.v_flip, mcfg.a_flip, 0);
                } 
                else {
                    ESP_LOGE(TAG, "未知指令: %s", action);
                    return -1;
                }
            } 
            else {
                ESP_LOGE(TAG, "无效动作指令: %s", action);
                return -1;
            }
        } 
        else {
            ESP_LOGE(TAG, "未知指令: %s", action);
            return -1;
        }
        if(ret != 0){
            return -1;
        }
        ESP_LOGI(TAG, "处理指令 %s, 耗时: %.1fms", action, (esp_timer_get_time() - start_time)/1000.0f);
    }
    ESP_LOGI(TAG, "总耗时: %.1fms", (esp_timer_get_time() - start_time_motions)/1000.0f);
    return 0;
}
void enable_all(void)
{
    uint8_t ids[] = {1, 2, 3, 4};
    if (cmd_enable(ids, 4, 1) == 0){
        ESP_LOGI(TAG, "使能成功");
    }else{
        ESP_LOGE(TAG, "使能失败");
    }
}
void disable_all(void)
{
    uint8_t ids[] = {1, 2, 3, 4};
    if (cmd_enable(ids, 4, 0) == 0){
        ESP_LOGI(TAG, "禁用成功");
    }else{
        ESP_LOGE(TAG, "禁用失败");
    }
}

void print_pos(void)
{
    for (int i = 1; i <= 4; i++) {
        cmd_get_pos(i);
        // 即使没有打印信息，也会更新g_angles[4]
        // ESP_LOGI(TAG, "电机 %d 位置: %ld", i, pos);
    }
}

// 检查通信稳定性
int motion_self_test(void) 
{
    int avg_voltage = 0, avg_temperature = 0;
    int error_count = 0;
    for(int count=0; count<16; count++){
        for (int i = 1; i <= 4; i++) {
            uint16_t cmd_count;
            uint8_t trap_status;
            int8_t temperature;
            int32_t pos;
            int16_t voltage;
            
            if (cmd_stat(i, &cmd_count, &trap_status, &temperature, &pos, &voltage) == 0) {
                avg_voltage += voltage;
                avg_temperature += temperature;
            }else{
                error_count ++;
                ESP_LOGE(TAG, "%d号电机通信不稳定", i);
            }
        }
    }
    if(error_count != 0){
        return -1;
    }else{
        avg_voltage /= 64;
        avg_temperature /= 64;
        ESP_LOGI(TAG, "完成电机通信自检，平均电压%.3fV, 平均温度%d℃", avg_voltage/1000.0f, avg_temperature);

        return 0;
    }
}